from machine import *
from smartcar import ticker,encoder
from seekfree import IMU660RX
from seekfree import MOTOR_CONTROLLER
import gc
import math
import time
from ccd2 import *
# 初始化硬件
led = Pin('C4', Pin.OUT, pull=Pin.PULL_UP_47K, value=True)
switch2 = Pin('D9', Pin.IN, pull=Pin.PULL_UP_47K, value=True)
state2 = switch2.value()
encoder_3 = encoder("C2", "C3", True)
encoder_4 = encoder("C0", "C1")
imu = IMU660RX()

# 全局变量
previous_acc = [0, 0, 0]  # 上一次的加速度计数据
previous_gyro = [0, 0, 0]  # 上一次的陀螺仪数据
ticker_flag = False
ticker_count = 0
data_ready = False

# PID控制器
class PID:
    def __init__(self, Kp, Kd, setpoint=0):
        self.Kp = Kp  # 比例系数
        self.Kd = Kd  # 微分系数
        self.setpoint = setpoint  # 目标值
        self.last_error = 0
        self.integral = 0

    def compute(self, measured_value):
        error = self.setpoint - measured_value  # 计算误差
        derivative = error - self.last_error  # 微分项计算
        output = (self.Kp * error) + (self.Kd * derivative)
        self.last_error = error
        return output
# 速度环 PID 控制器类
class VelocityPIDController:
    def __init__(self, Velocity_p, Velocity_i, a, target):
        self.Velocity_p = Velocity_p
        self.Velocity_i = Velocity_i
        self.a = a
        self.target = target
        self.pwm_out = 0
        self.error = 0
        self.encoder_l = 0
        self.encoder_r = 0
        self.EnC_Err_Lowout = 0
        self.EnC_Err_Lowout_last = 0
        self.Encoder_S = 0
        self.integral_limit = 1000  # 积分限幅

    def compute(self, encoder_3, encoder_4):
        self.encoder_l = encoder_3.get()
        self.encoder_r = encoder_4.get()
        self.error = (self.encoder_l + self.encoder_r)/2-self.target 
        # 低通滤波
        self.EnC_Err_Lowout = (1 - self.a) * self.error + self.a * self.EnC_Err_Lowout_last
        self.EnC_Err_Lowout_last = self.EnC_Err_Lowout
        # 积分累加
        self.Encoder_S += self.EnC_Err_Lowout
        # 积分限幅
        if self.Encoder_S > self.integral_limit:
            self.Encoder_S = self.integral_limit
        elif self.Encoder_S < -self.integral_limit:
            self.Encoder_S = -self.integral_limit

        self.pwm_out = self.Velocity_p * self.error + self.Velocity_i * self.Encoder_S
        return self.pwm_out

def low_pass_filter(current_value, previous_value, alpha=0.9):
    return alpha * previous_value + (1 - alpha) * current_value

# IMU数据结构
class GyroOffsetTypeDef:
    def __init__(self):
        self.gyroOffsetX = 0.0
        self.gyroOffsetY = 0.0
        self.gyroOffsetZ = 0.0

class IMUDataTypeDef:
    def __init__(self):
        self.accX = 0.0
        self.accY = 0.0
        self.accZ = 0.0
        self.gyroX = 0.0
        self.gyroY = 0.0
        self.gyroZ = 0.0

class QuaterTypedef:
    def __init__(self):
        self.q0 = 1.0
        self.q1 = 0.0
        self.q2 = 0.0
        self.q3 = 0.0

class EulerTypedef:
    def __init__(self):
        self.pitch = 0.0
        self.roll = 0.0
        self.yaw = 0.0

m_gyroOffset = GyroOffsetTypeDef()
g_imuData = IMUDataTypeDef()
m_Q = QuaterTypedef()
g_eulerAngle = EulerTypedef()

# IMU初始化
def IMU_gyro_offset_init():
    global m_gyroOffset
    m_gyroOffset.gyroOffsetX = 0.0
    m_gyroOffset.gyroOffsetY = 0.0
    m_gyroOffset.gyroOffsetZ = 0.0

    for i in range(1000):  # 增加采样点
        imu660ra_get_gyro()
        m_gyroOffset.gyroOffsetX += imu660ra_gyro_x
        m_gyroOffset.gyroOffsetY += imu660ra_gyro_y
        m_gyroOffset.gyroOffsetZ += imu660ra_gyro_z
        time.sleep_ms(10)

    m_gyroOffset.gyroOffsetX /= 1000
    m_gyroOffset.gyroOffsetY /= 1000
    m_gyroOffset.gyroOffsetZ /= 1000

# 获取IMU数据
def IMU_get_values():
    global g_imuData, m_gyroOffset

    imu660ra_get_acc()
    g_imuData.accX = imu660ra_acc_x * 0.001953
    g_imuData.accY = imu660ra_acc_y * 0.001953
    g_imuData.accZ = imu660ra_acc_z * 0.001953

    imu660ra_get_gyro()
    g_imuData.gyroX = (imu660ra_gyro_x - m_gyroOffset.gyroOffsetX) * 0.001064
    g_imuData.gyroY = (imu660ra_gyro_y - m_gyroOffset.gyroOffsetY) * 0.001064
    g_imuData.gyroZ = (imu660ra_gyro_z - m_gyroOffset.gyroOffsetZ) * 0.001064

# 四元数更新与欧拉角计算
def IMU_AHRSupdate(gx, gy, gz, ax, ay, az):
    global m_Q, g_eulerAngle

    HalfT = 0.001  # 2ms计算一次
    AHRS_Kp = 0.7  # 加速度计的收敛速率比例增益
    AHRS_Ki = 0.0071  # 陀螺仪收敛速率的积分增益

    # 归一化四元数
    norm = math.sqrt(m_Q.q0 * m_Q.q0 + m_Q.q1 * m_Q.q1 + m_Q.q2 * m_Q.q2 + m_Q.q3 * m_Q.q3)
    m_Q.q0 /= norm
    m_Q.q1 /= norm
    m_Q.q2 /= norm
    m_Q.q3 /= norm

    # 计算重力单位向量
    vx = 2 * (m_Q.q1 * m_Q.q3 - m_Q.q0 * m_Q.q2)
    vy = 2 * (m_Q.q0 * m_Q.q1 + m_Q.q2 * m_Q.q3)
    vz = m_Q.q0 * m_Q.q0 - m_Q.q1 * m_Q.q1 - m_Q.q2 * m_Q.q2 + m_Q.q3 * m_Q.q3

    # 计算误差
    ex = ay * vz - az * vy
    ey = az * vx - ax * vz
    ez = ax * vy - ay * vx

    # 更新角速度
    gx += (AHRS_Kp * ex)
    gy += (AHRS_Kp * ey)
    gz += (AHRS_Kp * ez)

    # 更新四元数
    m_Q.q0 += (-m_Q.q1 * gx - m_Q.q2 * gy - m_Q.q3 * gz) * HalfT
    m_Q.q1 += (m_Q.q0 * gx + m_Q.q2 * gz - m_Q.q3 * gy) * HalfT
    m_Q.q2 += (m_Q.q0 * gy - m_Q.q1 * gz + m_Q.q3 * gx) * HalfT
    m_Q.q3 += (m_Q.q0 * gz + m_Q.q1 * gy - m_Q.q2 * gx) * HalfT

    # 计算欧拉角
    sin_pitch = -2 * m_Q.q1 * m_Q.q3 + 2 * m_Q.q0 * m_Q.q2
    sin_pitch = max(-1.0, min(1.0, sin_pitch))  # 限制在 [-1, 1] 范围内
    g_eulerAngle.pitch = math.asin(sin_pitch) * 57.296  # pitch
    g_eulerAngle.roll = math.atan2(2 * m_Q.q2 * m_Q.q3 + 2 * m_Q.q0 * m_Q.q1, -2 * m_Q.q1 * m_Q.q1 - 2 * m_Q.q2 * m_Q.q2 + 1) * 57.296  # roll
    g_eulerAngle.yaw = math.atan2(2 * m_Q.q1 * m_Q.q2 + 2 * m_Q.q0 * m_Q.q3, -2 * m_Q.q2 * m_Q.q2 - 2 * m_Q.q3 * m_Q.q3 + 1) * 57.296  # yaw

# 获取欧拉角
def IMU_get_euler():
    IMU_get_values()
    IMU_AHRSupdate(g_imuData.gyroX, g_imuData.gyroY, g_imuData.gyroZ, g_imuData.accX, g_imuData.accY, g_imuData.accZ)

# 获取加速度计数据
def imu660ra_get_acc():
    global imu660ra_acc_x, imu660ra_acc_y, imu660ra_acc_z, previous_acc
    imu_data = imu.get()
    current_acc = [imu_data[0], imu_data[1], imu_data[2]]
    imu660ra_acc_x = low_pass_filter(current_acc[0], previous_acc[0])
    imu660ra_acc_y = low_pass_filter(current_acc[1], previous_acc[1])
    imu660ra_acc_z = low_pass_filter(current_acc[2], previous_acc[2])
    previous_acc = [imu660ra_acc_x, imu660ra_acc_y, imu660ra_acc_z]

# 获取陀螺仪数据
def imu660ra_get_gyro():
    global imu660ra_gyro_x, imu660ra_gyro_y, imu660ra_gyro_z, previous_gyro
    imu_data = imu.get()
    current_gyro = [imu_data[3], imu_data[4], imu_data[5]]
    imu660ra_gyro_x = low_pass_filter(current_gyro[0], previous_gyro[0])
    imu660ra_gyro_y = low_pass_filter(current_gyro[1], previous_gyro[1])
    imu660ra_gyro_z = low_pass_filter(current_gyro[2], previous_gyro[2])
    previous_gyro = [imu660ra_gyro_x, imu660ra_gyro_y, imu660ra_gyro_z]

# 定时器中断处理
def time_pit_handler(time):
    global ticker_flag, ticker_count, data_ready
    ticker_flag = True
    ticker_count += 1
    imu660ra_get_acc()
    imu660ra_get_gyro()
    data_ready = True

# 初始化定时器
pit1 = ticker(1)
pit1.callback(time_pit_handler)
pit1.start(1)  # 10ms触发一次
motor_dir = 1
motor_duty = 0
pit1.capture_list(imu, encoder_3, encoder_4)
motor_1 = MOTOR_CONTROLLER(MOTOR_CONTROLLER.PWM_C28_DIR_C29, 13000, duty=0, invert=True)
motor_2 = MOTOR_CONTROLLER(MOTOR_CONTROLLER.PWM_C30_DIR_C31, 13000, duty=0, invert=True)
pid_output=0
angle_error=0
# 初始化IMU
IMU_gyro_offset_init()
velocity_pid = VelocityPIDController(Velocity_p=-0.6, Velocity_i=0.0, a=0.9, target=8)
angle_pid = PID(Kp=4.78*0.6, Kd=8*0.8, setpoint=23.50)  # 原有的角度环PID
gyro_pid = PID(Kp=-142, Kd=0, setpoint=0)  # 新增的角速度环PID
while True:
    IMU_get_euler()
    if ticker_flag and data_ready:
        if ticker_flag and (ticker_count % 20 == 0):
            velocity_output = velocity_pid.compute(encoder_3, encoder_4)
            angle_pid.setpoint = 23.50 + velocity_output
            angle_pid.setpoint = max(15, min(30, angle_pid.setpoint))
        if ticker_flag and (ticker_count % 3 == 0):
            angle_error = angle_pid.compute(g_eulerAngle.roll)
        if ticker_flag and (ticker_count % 2 == 0):
            pid_output = gyro_pid.compute(angle_error - g_imuData.gyroX)
        pid_output = max(-8000, min(8000, pid_output))

        # 电机控制
        if pid_output >= 0:
            motor_dir = 0  
        else:
            motor_dir = 1  
            
        motor_1.duty(pid_output+700)
        motor_2.duty(pid_output+900)

        # 调试输出
        print("Angle:{:>6.1f}°, GyroX:{:>6.1f}°/s, Out:{:>6d}".format(
            g_eulerAngle.roll, g_imuData.gyroX, int(pid_output)))
        
        ticker_flag = False
        data_ready = False

    if switch2.value() != state2:
        pit1.stop()
        print("Program terminated by user.")
        break

    gc.collect()







